
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       rc_channel.c
  * @author     baiyang
  * @date       2021-8-8
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "rc_channel.h"
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/*
  Return true if value is between lower and upper bound inclusive.
  False otherwise.
*/
static bool is_bounded_int32(int32_t value, int32_t lower_bound, int32_t upper_bound)
{
    if ((lower_bound <= upper_bound) &&
        (value >= lower_bound) && (value <= upper_bound)) {
        return true;
    }

    return false;
}

/**
  * @brief       
  * @param[in]   pRc  
  * @param[out]  
  * @retval      
  * @note        
  */
void RC_init(RC_HandleTypeDef *pRc)
{
    pRc->switch_state.current_position = -1;
    pRc->switch_state.debounce_position = -1;
    pRc->switch_state.last_edge_time_ms = 0;
}

/**
  * @brief       
  * @param[in]   pRc  
  * @param[out]  
  * @retval      
  * @note        
  */
void RC_recompute_pwm_no_deadzone(RC_HandleTypeDef *pRc)
{
    if (pRc->type_in == RC_CHANNEL_TYPE_RANGE) {
        pRc->control_in = RC_pwm_to_range_dz(pRc,0);
    } else {
        //RC_CHANNEL_ANGLE
        pRc->control_in = RC_pwm_to_angle_dz(pRc,0);
    }
}

/**
  * @brief       返回摇杆中位的值
  * @param[in]   pRc  
  * @param[out]  
  * @retval      
  * @note        
  */
int16_t RC_get_control_mid(RC_HandleTypeDef *pRc)
{
    if (pRc->type_in == RC_CHANNEL_TYPE_RANGE) {
        int16_t r_in = (pRc->_radio_min + pRc->_radio_max)/2;

        if (pRc->_reversed) {
            r_in = pRc->_radio_max - (r_in - pRc->_radio_min);
        }

        int16_t radio_trim_low  = pRc->_radio_min + pRc->_dead_zone;

        return (((int32_t)(pRc->high_in) * (int32_t)(r_in - radio_trim_low)) / (int32_t)(pRc->_radio_max - radio_trim_low));
    } else {
        return 0;
    }
}

/**
  * @brief       将脉冲宽度调制值转换为配置范围内的值
  * @param[in]   pRc  
  * @param[out]  
  * @retval      
  * @note        
  */
int16_t RC_pwm_to_range(RC_HandleTypeDef *pRc)
{
    return RC_pwm_to_range_dz(pRc,pRc->_dead_zone);
}

/**
  * @brief       使用指定的死区将脉冲宽度调制值转换为配置范围内的值
  * @param[in]   pRc  
  * @param[in]   _dead_zone  
  * @param[out]  
  * @retval      
  * @note        
  */
int16_t RC_pwm_to_range_dz(RC_HandleTypeDef *pRc, uint16_t _dead_zone)
{
    int16_t r_in = math_constrain_int16(pRc->radio_in, pRc->_radio_min, pRc->_radio_max);

    if (pRc->_reversed) {
        r_in = pRc->_radio_max - (r_in - pRc->_radio_min);
    }

    int16_t radio_trim_low  = pRc->_radio_min + _dead_zone;

    if (r_in > radio_trim_low) {
        return (((int32_t)(pRc->high_in) * (int32_t)(r_in - radio_trim_low)) / (int32_t)(pRc->_radio_max - radio_trim_low));
    }
    return 0;
}

/**
  * @brief       从当前radio_in值返回一个“摄氏角度（角度*100）”（通常为-4500到4500）
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
int16_t RC_pwm_to_angle(RC_HandleTypeDef *pRc)
{
    return RC_pwm_to_angle_dz(pRc, pRc->_dead_zone);
}

/**
  * @brief       使用指定的dead_zone从当前radio_in值返回一个“摄氏角度（角度*100）”（通常为-4500到4500）
  * @param[in]   pRc  
  * @param[in]   _dead_zone  
  * @param[out]  
  * @retval      
  * @note        
  */
int16_t RC_pwm_to_angle_dz(RC_HandleTypeDef *pRc, uint16_t _dead_zone)
{
    return RC_pwm_to_angle_dz_trim(pRc, _dead_zone, pRc->_radio_trim);
}

/**
  * @brief       使用指定的dead_zone从当前radio_in值返回一个“摄氏角度（角度*100）”（通常为-4500到4500）
  * @param[in]   pRc  
  * @param[in]   _dead_zone  
  * @param[in]   _trim  
  * @param[out]  
  * @retval      
  * @note        
  */
int16_t RC_pwm_to_angle_dz_trim(RC_HandleTypeDef *pRc, uint16_t _dead_zone, uint16_t _trim)
{
    int16_t radio_trim_high = _trim + _dead_zone;
    int16_t radio_trim_low  = _trim - _dead_zone;

    int16_t reverse_mul = (pRc->_reversed?-1:1);

    // don't allow out of range values
    int16_t r_in = math_constrain_int16(pRc->radio_in, pRc->_radio_min, pRc->_radio_max);

    if (r_in > radio_trim_high && pRc->_radio_max != radio_trim_high) {
        return reverse_mul * ((int32_t)pRc->high_in * (int32_t)(r_in - radio_trim_high)) / (int32_t)(pRc->_radio_max  - radio_trim_high);
    } else if (r_in < radio_trim_low && radio_trim_low != pRc->_radio_min) {
        return reverse_mul * ((int32_t)pRc->high_in * (int32_t)(r_in - radio_trim_low)) / (int32_t)(radio_trim_low - pRc->_radio_min);
    } else {
        return 0;
    }
}

/**
  * @brief       
  * @param[in]   pRc  
  * @param[out]  
  * @retval      
  * @note        
  */
float RC_norm_input(RC_HandleTypeDef *pRc)
{
    float ret;
    int16_t reverse_mul = (pRc->_reversed?-1:1);
    if (pRc->radio_in < pRc->_radio_trim) {
        if (pRc->_radio_min >= pRc->_radio_trim) {
            return 0.0f;
        }
        ret = reverse_mul * (float)(pRc->radio_in - pRc->_radio_trim) / (float)(pRc->_radio_trim - pRc->_radio_min);
    } else {
        if (pRc->_radio_max <= pRc->_radio_trim) {
            return 0.0f;
        }
        ret = reverse_mul * (float)(pRc->radio_in - pRc->_radio_trim) / (float)(pRc->_radio_max  - pRc->_radio_trim);
    }
    return math_constrain_float(ret, -1.0f, 1.0f);
}

// centered around the channel trim. Take into account the deadzone
float RC_norm_input_dz(RC_HandleTypeDef *pRc)
{
    int16_t dz_min = pRc->_radio_trim - pRc->_dead_zone;
    int16_t dz_max = pRc->_radio_trim + pRc->_dead_zone;
    float ret;
    int16_t reverse_mul = (pRc->_reversed?-1:1);
    if (pRc->radio_in < dz_min && dz_min > pRc->_radio_min) {
        ret = reverse_mul * (float)(pRc->radio_in - dz_min) / (float)(dz_min - pRc->_radio_min);
    } else if (pRc->radio_in > dz_max && pRc->_radio_max > dz_max) {
        ret = reverse_mul * (float)(pRc->radio_in - dz_max) / (float)(pRc->_radio_max  - dz_max);
    } else {
        ret = 0;
    }
    return math_constrain_float(ret, -1.0f, 1.0f);
}

// return a normalised input for a channel, in range -1 to 1,
// ignores trim and deadzone
float RC_norm_input_ignore_trim(RC_HandleTypeDef *pRc)
{
    // sanity check min and max to avoid divide by zero
    if (pRc->_radio_max <= pRc->_radio_min) {
        return 0.0f;
    }
    const float ret = (pRc->_reversed ? -2.0f : 2.0f) * (((float)(pRc->radio_in - pRc->_radio_min) / (float)(pRc->_radio_max - pRc->_radio_min)) - 0.5f);
    return math_constrain_float(ret, -1.0f, 1.0f);
}

/*
  get percentage input from 0 to 100. This ignores the trim value.
 */
uint8_t RC_percent_input(RC_HandleTypeDef *pRc)
{
    if (pRc->radio_in <= pRc->_radio_min) {
        return pRc->_reversed?100:0;
    }
    if (pRc->radio_in >= pRc->_radio_max) {
        return pRc->_reversed?0:100;
    }
    uint8_t ret = 100.0f * (pRc->radio_in - pRc->_radio_min) / (float)(pRc->_radio_max -pRc->_radio_min);
    if (pRc->_reversed) {
        ret = 100 - ret;
    }

    return ret;
}

// return true if input is within deadzone of trim
bool RC_in_trim_dz(RC_HandleTypeDef *pRc)
{
    return is_bounded_int32(pRc->radio_in, pRc->_radio_trim - pRc->_dead_zone, pRc->_radio_trim + pRc->_dead_zone);
}

/**
  * @brief       
  * @param[in]   pRc  
  * @param[out]  
  * @retval      
  * @note        
  */
void RC_clear_override(RC_HandleTypeDef *pRc)
{
    pRc->last_override_time = 0;
    pRc->override_value = 0;
}

/*
  perform stick mixing on one channel
  This type of stick mixing reduces the influence of the auto
  controller as it increases the influence of the users stick input,
  allowing the user full deflection if needed
 */
int16_t RC_stick_mixing(RC_HandleTypeDef *pRc, const int16_t servo_in)
{
    float ch_inf = (float)(pRc->radio_in - pRc->_radio_trim);
    ch_inf = fabsf(ch_inf);
    ch_inf = MIN(ch_inf, 400.0f);
    ch_inf = ((400.0f - ch_inf) / 400.0f);

    int16_t servo_out = servo_in;
    servo_out *= ch_inf;
    servo_out += pRc->control_in;

    return servo_out;
}

// get control input with zero deadzone
int16_t RC_get_control_in_zero_dz(RC_HandleTypeDef *pRc)
{
    if (pRc->type_in == RC_CHANNEL_TYPE_RANGE) {
        return RC_pwm_to_range_dz(pRc, 0);
    }
    return RC_pwm_to_angle_dz(pRc, 0);
}

/**
  * @brief       
  * @param[in]   pRc  
  * @param[out]  
  * @retval      
  * @note        
  */
void RC_reset_mode_switch(RC_HandleTypeDef *pRc)
{
    pRc->switch_state.current_position = -1;
    pRc->switch_state.debounce_position = -1;
    RC_read_mode_switch(pRc);
}

void RC_read_mode_switch(RC_HandleTypeDef *pRc)

{
    // calculate position of flight mode switch
    const uint16_t pulsewidth = pRc->radio_in;
    if (pulsewidth <= 900 || pulsewidth >= 2200) {
        return;  // This is an error condition
    }

    modeswitch_pos_t position;
    if      (pulsewidth < 1231) position = 0;
    else if (pulsewidth < 1361) position = 1;
    else if (pulsewidth < 1491) position = 2;
    else if (pulsewidth < 1621) position = 3;
    else if (pulsewidth < 1750) position = 4;
    else position = 5;

    if (!RC_debounce_completed(pRc, (int8_t)position)) {
        return;
    }

    // set flight mode and simple mode setting
    RC_mode_switch_changed(pRc, position);
}

/**
  * @brief       
  * @param[in]   pRc  
  * @param[in]   high  
  * @param[out]  
  * @retval      
  * @note        
  */
void RC_set_range(RC_HandleTypeDef *pRc, uint16_t high)
{
    pRc->type_in = RC_CHANNEL_TYPE_RANGE;
    pRc->high_in = high;
}

/**
  * @brief       
  * @param[in]   pRc  
  * @param[out]  
  * @retval      
  * @note        
  */
uint16_t RC_get_range(RC_HandleTypeDef *pRc)
{
    return pRc->high_in;
}

/**
  * @brief       
  * @param[in]   pRc  
  * @param[in]   angle  
  * @param[out]  
  * @retval      
  * @note        
  */
void RC_set_angle(RC_HandleTypeDef *pRc, uint16_t angle)
{
    pRc->type_in = RC_CHANNEL_TYPE_ANGLE;
    pRc->high_in = angle;
}

/**
  * @brief       
  * @param[in]   pRc  
  * @param[out]  
  * @retval      
  * @note        
  */
bool RC_get_reverse(RC_HandleTypeDef *pRc)
{
    return (bool)pRc->_reversed;
}

/**
  * @brief       
  * @param[in]   pRc  
  * @param[in]   dzone  
  * @param[out]  
  * @retval      
  * @note        
  */
void RC_set_default_dead_zone(RC_HandleTypeDef *pRc, int16_t dzone)
{
    pRc->_dead_zone = dzone;
}

/**
  * @brief       
  * @param[in]   pRc  
  * @param[out]  
  * @retval      
  * @note        
  */
uint16_t RC_get_dead_zone(RC_HandleTypeDef *pRc)
{
    return pRc->_dead_zone;
}

/*------------------------------------test------------------------------------*/


